/**
 * @file app.cpp
 * @Application wrapper
 * @author  Bui Van Hieu <bvhieu@cse.hcmut.edu.vn>
 * @version 1.0
 * @date 01-09-2013
 *
 * @copyright
 * This project and all its relevant hardware designs, documents, source codes, compiled libraries
 * belong to <b> Smart Sensing and Intelligent Controlling Group (SSAIC Group)</b>. 
 * You have to comply with <b> Non-Commercial Share-Alike Creative Common License </b> 
 * in order to share (copy, distribute, transmit) or remix (modify, reproduce, adapt) these works.\n
 * SSAIC Group shall not be held liable for any direct, indirect or consequential damages 
 * with respect to any claims arising from the content of hardware, firmware and/or the use 
 * made by customers of the coding information contained herein in connection with their products.\n
 * You are prohibited from commercializing in any kind that using or basing on these works
 * without written permission from SSAIC Group. Please contact ssaic@googlegroups.com for commercializing
 */
#include "hlib.h"
#include "AX_12.cpp"

#include <stdio.h>
uint32_t counter;
void Delay(__IO uint32_t num);
void Demo1();
void Demo2();
void Demo3();
void Demo4();
void Demo5();
void snack();
//=======================================================
void Setup(void){
  counter = 0;
  /* ADD CODE HERE TO INITIALIZE YOUR PROGRAM */
  COM2.Start(1000000);

}



//=======================================================
void Loop(void){

  /* ADD CODE HERE TO IMPLEMENT YOUR INFINITIVE LOOP  */
	snack();

	return;
}

void Delay(__IO uint32_t num)
{
	__IO uint32_t index = 0;

	/* default system clock is 72MHz */
	for(index = (720000 * num); index != 0; index--)
	{
	}
}

void Demo1()
{
	/*  uint8_t  getChar;
  if (COM1.HasData()){
    getChar = COM1.Get();
    switch (getChar){
      case 'c': LCD.Clear(); break;
      case 'h': LCD.Home(); break;
      case 'o': LCD.OnOff(true, false, false); break;
      case 'n': LCD.Print(1234, 10);
      case '0': LCD.Goto(0, 0); break;
      case '1': LCD.Goto(1, 1); break;
      default: LCD.Print((char) getChar);
    }
  } */

	AX12.SetMovingSpeed(1, 0x300);
	AX12.SetGoalPosition(1, 0x300); Delay(7);
	AX12.SetGoalPosition(1, 0x100); Delay(7);
	AX12.SetMovingSpeed(1, 0x010);
	AX12.SetGoalPosition(1, 0x300); Delay(60);
	AX12.SetGoalPosition(1, 0x200); Delay(25);	
	AX12.SetLed(1, 1); Delay(2);	
	AX12.SetLed(1, 0); Delay(2);	
	AX12.SetLed(1, 1); 
	AX12.SetID(1, 2);

	for(int i=0; i<10; i++){
		AX12.SetLed(2, 1); Delay(1);
		AX12.SetLed(2, 0); Delay(1);
		AX12.SetMovingSpeed(2, 0x100);
		AX12.SetGoalPosition(2, 0x150); Delay(5);
		AX12.SetGoalPosition(2, 0x250); Delay(5);
	}
	AX12.SetID(2,1);

	return;
}

void Demo2()
{
	int servoId = 1;
	int alarmLed = 1;
	int alarmShutdown = 1;
	AX12.SetAlarmLed(servoId, alarmLed); Delay(10);
	AX12.SetAlarmShutdown(servoId, alarmShutdown); Delay(10);
	return;
}

void Demo3()
{
	AX12.SetMovingSpeed(1, 0x010);
	AX12.SetGoalPosition(1, 0x100); 
	AX12.SetGoalPosition(2, 0x300); Delay(10);
	AX12.SetGoalPosition(1, 0x300); Delay(5);
}

void Demo4()
{
	uint16_t	value_1[] = {1, 0x0ff, 0x030, 2, 0x0ff, 0x030};
	uint16_t	value_2[] = {1, 0x1ff, 0x030, 2, 0x1ff, 0x030};
	AX12.SetGoalPosition(1, 0x050); Delay(10);
	AX12.SetGoalPosition(1, 0x300); Delay(10);
}

void snack()
{
	// add code here to control the snack
	// can use all functions 
/*	AX12.SetLed(1, 1); Delay(5);
	AX12.SendReset(1);
	AX12.SendWriteRegTwoBytes(2, AX12_Goal_Position_L_REG, 0x100);
	AX12.SetLed(2, 1); Delay(5);
	AX12.SendAction(2);
	AX12.SendWriteRegTwoBytes(2, AX12_Goal_Position_L_REG, 0x300);*/
	AX12.SetLed(2, 0);
	uint8_t value[] = {1};
	AX12.SendWriteDataMultiBytes(2, 0x19, value, 1);Delay(10);
	uint8_t value1[] = {0xFF, 0x03};
	AX12.SendWriteDataMultiBytes(2, 0x1E, value1, 2);Delay(10);
}